#include "bsp_motor.h"

uint8_t direction = LAST_L;

void Motor_Init()
{
    pinMode(MOTOR_LEFT_1, OUTPUT);
    pinMode(MOTOR_LEFT_2, OUTPUT);
    pinMode(MOTOR_RIGHT_1, OUTPUT);
    pinMode(MOTOR_RIGHT_2, OUTPUT);

    analogWrite(MOTOR_LEFT_1, STOP);
    analogWrite(MOTOR_LEFT_2, STOP);
    analogWrite(MOTOR_RIGHT_1, STOP);
    analogWrite(MOTOR_RIGHT_2, STOP);
}

/**
 * @brief 电机全速 中速 低速前进
 * @param pwm  val: @enum Speed
 * @return void
 * */
void Forward(Speed pwm)
{
    if(pwm + L_R_CALIB > 240 )
        pwm = 240 - L_R_CALIB;

    Motor_SetPWM(pwm, pwm + L_R_CALIB);
}

/**
 * @brief 电机全速 中速 低速后退
 * @param pwm  val: @enum Speed
 * @return void
 * */
void Backward(Speed pwm)
{
    if(pwm + L_R_CALIB > 240 )
        pwm = 240 - L_R_CALIB;

    Motor_SetPWM(-pwm, -(pwm + L_R_CALIB));
}

/**
 * @brief 电机差速左转
 * @param Speed pwm, Speed offset
 * @return void
 * */
void turnLeft_FW(Speed pwm,Speed offset)
{

    if((pwm+offset) > 240) 
        offset = 240 - pwm ;
    

    Motor_SetPWM(pwm,pwm+offset+L_R_CALIB);

}

/**
 * @brief 电机顺时针旋转 差速前进右转 微调前进右转
 * @param pwm  val: @enum Speed
 * @return void
 * */
void turnRight_FW(Speed pwm,Speed offset)
{
    if((pwm+offset) > 240) 
        offset = 240 - pwm ;
       
    Motor_SetPWM(pwm+offset,pwm+L_R_CALIB);

}

/**
 * @brief 电机顺时针旋转 差速后退左转 微调后退左转
 * @param pwm  val: @enum Speed
 * @return void
 * */
void turnLeft_BW(Speed pwm,Speed offset)
{

    if((pwm+offset) > 240) 
        offset = 240 - pwm ;
    
    Motor_SetPWM(-(pwm+offset),-(pwm+L_R_CALIB));


}

/**
 * @brief 电机顺时针旋转 差速后退右转 微调后退右转
 * @param pwm  val: @enum Speed
 * @return void
 * */
void turnRight_BW(Speed pwm,Speed offset)
{
    if((pwm+offset) > 240) 
        offset = 240 - pwm ;
    
    Motor_SetPWM(-pwm,-(pwm+offset+L_R_CALIB));

}
/**
 * @brief 电机逆时针旋转
 * @param Speed PWM
 * @return void
 * */
void turnRound_L(Speed pwm)
{
    if(pwm + L_R_CALIB > 240 )
        pwm = 240 - L_R_CALIB;

    Motor_SetPWM(-pwm,pwm+L_R_CALIB);  
    direction = LAST_L;
}

/**
 * @brief 电机顺时针旋转
 * @param Speed PWM
 * @return void
 * */
void turnRound_R(Speed pwm)
{
    if(pwm + L_R_CALIB > 240 )
        pwm = 240 - L_R_CALIB;

    Motor_SetPWM(pwm,-(pwm+L_R_CALIB));  
    direction = LAST_R;
}
/**
 * @brief 电机刹车
 * @param void
 * @return void
 * */
void brack()
{
  Motor_SetPWM(0,0);
}
